#include "include.h"
#include <math.h>
#define MB_ADRC_ABS(x)                   ((x)>=0?(x):-(x))
#define MB_ADRC_SIGN(x)                  ((x)>=0?1:-1)
#define MB_ADRC_LIMIT(x,min,max)         (((x)<=(min) ? (min) : ((x)>=(max) ? (max) : (x))))

_MB_ADRC_FHAN_T MB_ADRC_fhan[5];

void MeBSP_ADRC_fhan_Init(void)
{
  uint8_t c;
  for ( c = 0; c < 5; c++ ) {
    memset(&MB_ADRC_fhan[c],0,sizeof(MB_ADRC_fhan));
    MB_ADRC_fhan[c].wTargetVal = 0;
    MB_ADRC_fhan[c].fH = 0.005f;
    MB_ADRC_fhan[c].fR = 400;
  } 
}

float MeBSP_ADRC_Msqrt(float number)
{
  long i;
  float x, y;
  x = number * 0.5f;
  y = number;
  i = *(long *)&y;
  i = 0x5f3759df - (i >> 1);
  y = *(float *)&i;
  y = y * (1.5f - (x * y * y));
  y = y * (1.5f - (x * y * y));
  return number * y;
}

float MeBSP_ADRC_fhan(float x1, float x2, float r,float h)
{
  float d,d0,y,a0,absy,a,u;
  if ( x1 == 0 ) {
    return 0;
  }
  d = r*h;
  d0 = h*d;
  y = x1 + h * x2;
  absy = MB_ADRC_ABS(y);
  a0 = MeBSP_ADRC_Msqrt(d*d + 8.0f*r*absy);
  
  if ( absy > d0 ) {
    a = x2+(a0 - d)/2.0f * MB_ADRC_SIGN(y);
  }
  else {
    a = x2 + y / h;
  }
  if ( MB_ADRC_ABS(a) > d ) {
    u = -r * MB_ADRC_SIGN(a);
  }
  else {
    u = -r * a / d;
  }
  return u;
}

void MeBSP_ADRC_fhan_Hanl(void)
{
  uint8_t c;
  float u;
  _MB_ADRC_FHAN_T *p;
  uint16_t *ptag_pwm;
  uint8_t curve_param_change;
  uint32_t val;
  curve_param_change = 0;
  if ( MB_PWM.wCurveParVal[0] != MB_DevComIn_Data.packet.wPwmCurveParam[0] ) {
    MB_PWM.wCurveParVal[0] = MB_DevComIn_Data.packet.wPwmCurveParam[0];
    curve_param_change = 1;
  }
  if ( MB_PWM.wCurveParVal[1] != MB_DevComIn_Data.packet.wPwmCurveParam[1] ) {
    MB_PWM.wCurveParVal[1] = MB_DevComIn_Data.packet.wPwmCurveParam[1];
    curve_param_change = 1;
  }

  if ( curve_param_change ) {
    for ( c = 0; c < 5; c++ ) {
      p = &MB_ADRC_fhan[c];
      p->fH = MB_PWM.wCurveParVal[1] * 0.0001f;
      p->fR = MB_PWM.wCurveParVal[0] * 0.1f;
    }
  }
  val  = MB_DevComIn_Data.packet.cLedNumVal;
  if ( val ) {
    val -= 1;
  }
  ptag_pwm = &MB_DevComIn_Data.packet.wPwmVal[val * 6];
  if ( MB_PWM.bDimmerDelayEnable ) {
    for ( c = 0; c < 5; c++ ) {
      p = &MB_ADRC_fhan[c];

      p->wTargetVal = ptag_pwm[c];
      u = MeBSP_ADRC_fhan(p->wActualVal-p->wTargetVal,p->fX2,p->fR,p->fH);
      p->fX2 += p->fH * u;
      u = p->wActualVal + p->fH * p->fX2;
      if ( MB_ADRC_ABS(p->wTargetVal - u)  < 0.5f ) {
        p->wActualVal = p->wTargetVal;
        p->fX2 = 0;
      }
      else {
        p->wActualVal = u;
      }
      if ( p->wActualVal < MB_DevComIn_Data.packet.wPwmStartOffVal[c] ) {
        MB_PWM.wNowVal[c] = 0;
      }
      else {
        MB_PWM.wNowVal[c] = p->wActualVal;
      }
      
    }
  }
  else {
    for ( c = 0; c < 5; c++ ) {
      p = &MB_ADRC_fhan[c];
      if ( ptag_pwm[c] < MB_DevComIn_Data.packet.wPwmStartOffVal[c] ) {
        MB_PWM.wNowVal[c] = 0;
      }
      else {
        MB_PWM.wNowVal[c] = ptag_pwm[c];
      }
      p->wActualVal = MB_PWM.wNowVal[c];
      p->fX2 = 0;
    }
  }

  

  // sprintf(buf, "%d\r\n", MB_PWM.wNowVal[0]);
  // USER_Debug_Out(buf);
  
  
}







